package nape.space;
import cx.FastList;
import cx.Algorithm;
import cx.Allocator;
import nape.shape.Shape;
import nape.shape.Polygon;
import nape.geom.AABB;
import nape.geom.Vec2;
import nape.geom.Ray;
import nape.geom.RayResult;
import nape.geom.VecMath;
import nape.phys.PhysObj;
import nape.phys.Particle;
import nape.space.Space;
import nape.dynamics.Collide;
import nape.Const;
import nape.Config;
import nape.util.FastMath;
import nape.util.Array2;
import nape.callbacks.Callback;




























class UniformCell {
	
	public var static_shapes :CxFastList(Shape);
	public var dynamic_shapes:CxFastList(Shape);
	public var particles     :CxFastList(Particle);
	public var dynamics      :CxFastList(Body);
	public var statics       :CxFastList(Body);
	
	public var stamp:Int;
	public var broad:Bool;
	
	
	
	
		
	public function new () {
		stamp = 0;
		broad = false;
		
		static_shapes  = new CxFastList(Shape)   ();
		dynamic_shapes = new CxFastList(Shape)   ();
		particles      = new CxFastList(Particle)();
		dynamics       = new CxFastList(Body)    ();
		statics        = new CxFastList(Body)    ();
	}
	
	
		
	public inline function clear() {
		dynamic_shapes.clear();
		particles.clear();
		dynamics.clear();
	}
	
}



class UniformSpace extends Space {
	
	public var cells:Array2<UniformCell>;
	public var broad:CxFastList(UniformCell);
	
	public var x0:Float;
	public var y0:Float;
	public var wid:Int; public var hei:Int;
	public var idim:Float; public var dim:Float;
	
	
	
	public function new (domain:AABB, cell_dimensions:Float, ?gravity:Vec2) {
		super(gravity);
		sleeping = false;
		
		x0 = domain.minx;
		y0 = domain.miny;
		idim = 1 / cell_dimensions;
		dim = cell_dimensions;
		wid = Math.ceil(domain.width() * idim);
		hei = Math.ceil(domain.height() * idim);
		
		cells = new Array2<UniformCell>(wid,hei);
		for (x in 0...wid) {
			for (y in 0...hei) 
				cells.set(x, y, new UniformCell());
		}
		
		broad = new CxFastList(UniformCell)();
		
		cellShape = new Polygon([new Vec2(0,0), new Vec2(dim,0), new Vec2(dim,dim), new Vec2(0,dim)], new Vec2());
	}
	
	
	
	
	
	
	public override function clear_special() {
		broad.clear();
		for(x in 0...wid) {
			for (y in 0...hei) {
				var cell = cells.get(x,y);
				cell.clear();
				cell.static_shapes.clear();
				cell.statics.clear();
				cell.broad = false;
			}
		}
	}
	
	
	
	public override function addObject(o:PhysObj):Void {
		if(o.added_to_space) return;
		add_aux(o);
		o.added_to_space = true;
		objects.add(o);
		
		o.visible = o.pvisible = false;
		
		if (o.isBody) {
			var body = o.body;
			if (body.isStatic = (body.smass == 0 && body.smoment == 0)) {
				addStatic(body);
				body.updatePosition(0);
				CxIter(s,body.shapes,{
					s.updateShape();
					syncStatic(s);
				});
			}
			else {
				addDynamic(body);
				CxIter(s,body.shapes,syncShape(s,false));
				addProperties(o.properties);
			}
			registerBody(body);
		}else {
			var part = o.particle;
			addParticle(part);
			syncParticle(part,false);
			addProperties(o.properties);
			registerBase(part);
		}
	}
	public override function removeObject(o:PhysObj):Void {
		if(!objects.remove(o)) return;
		rem_aux(o);
		o.added_to_space = false;
		
		if(o.pvisible || o.visible) {
			visible.remove(o);
			physobj_cb(o,Hide,HIDE);
		}
		o.visible = o.pvisible = false;
		
		if (o.isBody) {
			var body = o.body;
			if (body.isStatic) {
				removeStatic(body);
				CxIter(s,body.shapes,{
					coord(s.aabb)
					for (v in v0...v1) {
						for (u in u0...u1) {
							var c = cells.get(u, v);
							if(c.static_shapes.remove(s)) {
								if(c.stamp<stamp) c.clear();
								broadcell(c);
								c.statics.remove(body);
							}
						}
					}
				});
			}else {
				removeDynamic(body);
				CxIter(s,body.shapes,{
					coord(s.aabb)
					for (v in v0...v1) {
						for (u in u0...u1) {
							var c = cells.get(u, v);
							if(c.stamp!=stamp) continue;
							
							if(c.dynamic_shapes.remove(s)) {
								c.dynamics.remove(body);
								broadcell(c);
							}
						}
					}
				});
				removeProperties(o.properties);
			}
		}else {
			var p = o.particle;
			removeParticle(p);
			coordp(p)
			for (u in u0...u1) {
				for (v in v0...v1) {
					var c = cells.get(u, v);
					if(c.stamp!=stamp) continue;
							
					if(c.particles.remove(p))
						broadcell(c);
				}
			}
			removeProperties(o.properties);
		}
		removeConstraints(o);
	}
	
	
	
	public var cellShape:Polygon;
	public inline function syncStatic(s:Shape):Void {
		coord(s.aabb)
		
		var pverts = cellShape.pverts;
		var aabb   = cellShape.aabb;
		var paxi   = cellShape.paxi;
		var laxi   = cellShape.laxi;
		
		for (u in u0...u1) {
			for (v in v0...v1) {
				var c = cells.get(u, v);
				
				var ax = x0+u*dim;
				var ay = y0+v*dim;
				
				var ite = pverts.begin();
				var p:Vec2;
				p = ite.elem(); vec_set(p.p,ax,ay);         ite = ite.next;
				p = ite.elem(); vec_set(p.p,ax,ay+dim);     ite = ite.next;
				p = ite.elem(); vec_set(p.p,ax+dim,ay+dim); ite = ite.next;
				p = ite.elem(); vec_set(p.p,ax+dim,ay);
				aabb.minx = ax;
				aabb.maxx = ax+dim;
				aabb.miny = ay;
				aabb.maxy = ay+dim;
				
				var lte = laxi.begin();
				CxIter(pax,paxi,{
					pax.d = vec_dot(a,pax.n) + lte.elem().d;
					lte = lte.next;
				});
				
				var b = s.body;
				if(!c.statics.has(b)) c.statics.add(b);
				
				if(!collide.testCollide(Shape,Shape)(s,cellShape)) continue;
				
				if (c.stamp < stamp) { c.clear(); c.stamp = stamp; }
				c.static_shapes.add(s);
				broadcell(c);
			}
		}
	}

	public override function syncShape(s:Shape,check:Bool):Bool {
		coord(s.aabb)
		var ret = true;
		for (u in u0...u1) {
			for (v in v0...v1) {
				var c = cells.get(u, v);
				if (c.stamp < stamp) { c.clear(); c.stamp = stamp; }
				
				if(!check || !c.dynamic_shapes.has(s))
					c.dynamic_shapes.add(s);
				
				var b = s.body;
				if(!c.dynamics.has(b)) c.dynamics.add(b);
					
				broadcell(c);
				ret = false;
			}
		}
		return ret;
	}
	public override function syncParticle(p:Particle,check:Bool):Bool {
		coordp(p)
		var ret = true;
		for (u in u0...u1) {
			for (v in v0...v1) {
				var c = cells.get(u, v);
				if (c.stamp < stamp) { c.clear(); c.stamp = stamp; }
				if(!check || !c.particles.has(p))
					c.particles.add(p);
				broadcell(c);
				ret = false;
			}
		}
		return ret;
	}
	
	public inline function broadcell(c:UniformCell):Void {
		if(!c.broad) {
			if (!c.static_shapes.empty()) {
				if(!c.dynamic_shapes.empty() || !c.particles.empty()) {
					c.broad = true;
					broad.add(c);
				}
			}else if (!c.dynamic_shapes.empty()) {
				if (c.dynamic_shapes.begin().next != c.dynamic_shapes.end() || !c.particles.empty()) {
					c.broad = true;
					broad.add(c);
				}
			}
		}
	}
	
	
	
	public override function broadphase():Void {
		while(!broad.empty()) {
			var c = broad.front();
			
			
			c.broad = false;
			CxIter(stat,c.static_shapes,{
				CxIter(dyn, c.dynamic_shapes, narrowPhase_SD(stat,dyn));
				CxIter(part,c.particles,      narrowPhase_SP(stat, part));
			});
			CxIter(i,c.dynamic_shapes,{
				CxIterIte(j,c.dynamic_shapes,cxiterator.next, narrowPhase_DD(i,j));
				CxIter(part, c.particles,                     narrowPhase_DP(i,part));
			});
			broad.pop();
		}
	}
	
	
	
	public override function objectAtPoint(x:Float,y:Float):PhysObj {
		var u = Std.int((x - x0) * idim);
		var v = Std.int((y - y0) * idim);
		if(u<0||v<0||u>=wid||v>=hei) return null;
		
		var c = cells.get(u,v);
		if(c.stamp<stamp) return null;
		
		var p = alloc.CxAlloc(Vec2)(); vec_set(p.p,);
		
		CxIter(s,c.dynamic_shapes,{
			var b = s.body;
			if(collide.bodyContains(b,p))
				return b;
		});
		CxIter(s,c.static_shapes,{
			var b = s.body;
			if(collide.bodyContains(b,p))
				return b;
		});
		CxIter(t,c.particles,{
			if(collide.partContains(t,p))
				return t;
		});
		
		return null;
	}
	
	
	
	
	
	public override function rayCast(r:Ray):RayResult {
		var mint:Float = RayCast.FAIL;
		var mino:PhysObj = null;
		var mins:Shape = null;
		vec_new(polyn,0,0);
		var tempn:Vec2 = new Vec2();
		
		var u0 = Math.floor((r.ax-x0)*idim);
		var v0 = Math.floor((r.ay-y0)*idim);
		
		if(u0<0||v0<0||u0>=wid||v0>=hei) {
			var int = false;
			var ct = 16.0;
			{
				var y1 = y0+hei*dim;
				var x1 = x0+wid*dim;
				if(r.vx>0) {
					var t = (x0-r.ax)/r.vx;
					if(t<ct && !(t<0||t>1)) {
						var y = r.ay+r.vy*t;
						if(!(y<y0||y>y1)) ct = t;
					}
				}else if(r.vx<0) {
					var t = (x1-r.ax)/r.vx;
					if(t<ct && !(t<0||t>1)) {
						var y = r.ay+r.vy*t;
						if(!(y<y0||y>y1)) ct = t;
					}
				}
				
				if(r.vy>0) {
					var t = (y0-r.ay)/r.vy;
					if(t<ct && !(t<0||t>1)) {
						var x = r.ax+r.vx*t;
						if(!(x<x0||x>x1)) ct = t;
					}
				}else if(r.vy<0) {
					var t = (y1-r.ay)/r.vy;
					if(t<ct && !(t<0||t>1)) {
						var x = r.ax+r.vx*t;
						if(!(x<x0||x>x1)) ct = t;
					}
				}
			}
			if(!(ct<0||ct>1)) {
				vec_addeq(r.a,r.v,ct);
				vec_muleq(r.v,(1-ct));
				u0 = Math.floor((r.ax-x0)*idim);
				v0 = Math.floor((r.ay-y0)*idim);
				if(u0==wid) u0 = wid-1;
				if(v0==hei) v0 = hei-1;
			}else return null;
		}
		
		var g0x = x0 + u0*dim;
		var g0y = y0 + v0*dim;
		
		var tx0 = Const.FMAX;
		var ty0 = Const.FMAX;
		var du = 0; var dv = 0;
		var dtx = 0.0;
		var dty = 0.0;
		if(r.vx>0) {
			du = 1;
			tx0 = (g0x+dim-r.ax)/r.vx;
			dtx = dim/r.vx;
		}else if(r.vx<0) {
			du = -1;
			tx0 = (g0x-r.ax)/r.vx;
			dtx = -dim/r.vx;
		}
		if(r.vy>0) {
			dv = 1;
			ty0 = (g0y+dim-r.ay)/r.vy;
			dty = dim/r.vy;
		}else if(r.vy<0) {
			dv = -1;
			ty0 = (g0y-r.ay)/r.vy;
			dty = -dim/r.vy;
		}
		
		var ct = 0.0; var cu = u0; var cv = v0;
		var tx = tx0-dtx;
		var ty = ty0-dty;
		
		var ret:RayResult = null;
		while(ct<1 && cu>=0 && cv>=0 && cu<wid && cv<hei) {
			var nt; var nu; var nv;
			var ntx = tx+dtx;
			var nty = ty+dty;
			if(ntx<nty) {
				nt = tx = ntx;
				nu = cu+du;
				nv = cv;
			}else {
				nt = ty = nty;
				nu = cu;
				nv = cv+dv;
			}
			if(nt>1) nt = 1;
			
			var cell = cells.get(cu,cv);
			if(cell.stamp==stamp) {
				if( {
		var mint:Float = RayCast.FAIL;
		var mino:PhysObj = null;
		var mins:Shape = null;
		vec_new(polyn,0,0);
		var tempn:Vec2 = new Vec2();
		
		
		if(!false)  CxIter(s,cell.dynamic_shapes,{
			var t = if(s.type == Shape.CIRCLE) RayCast.rayCircle (r,s.circle );
			        else                       RayCast.rayPolygon(r,s.polygon,tempn);
			if(t<mint) {
				mint = t;
				mino = s.body;
				mins = s;
				vec_set(polyn,tempn.p);
			}
		});
		 CxIter(s,cell.static_shapes,{
			var t = if(s.type == Shape.CIRCLE) RayCast.rayCircle (r,s.circle );
			        else                       RayCast.rayPolygon(r,s.polygon,tempn);
			if(t<mint) {
				mint = t;
				mino = s.body;
				mins = s;
				vec_set(polyn,tempn.p);
			}
		});
		
		var retv;
		if(!(mint<ct||mint>nt) && mint!=RayCast.FAIL) {
			if(ret==null) ret = new RayResult();
			ret.obj = mino;
			ret.shape = mins;
			vec_set(ret.p,r.a);
			vec_addeq(ret.p,r.v,mint);
			if(ret.shape.type == Shape.CIRCLE) {
				vec_sub(ret.p,ret.shape.circle.centre.p,ret.n);
				vec_normalise(ret.n);
			}else {
				vec_set(ret.n,polyn);
			}
			ret.t = mint;
			retv = true;
		}else retv = false;
		
		retv;
	})
					return ret;
			}else if(!cell.static_shapes.empty()) {
				if( {
		var mint:Float = RayCast.FAIL;
		var mino:PhysObj = null;
		var mins:Shape = null;
		vec_new(polyn,0,0);
		var tempn:Vec2 = new Vec2();
		
		
		if(!true)  CxIter(s,cell.dynamic_shapes,{
			var t = if(s.type == Shape.CIRCLE) RayCast.rayCircle (r,s.circle );
			        else                       RayCast.rayPolygon(r,s.polygon,tempn);
			if(t<mint) {
				mint = t;
				mino = s.body;
				mins = s;
				vec_set(polyn,tempn.p);
			}
		});
		 CxIter(s,cell.static_shapes,{
			var t = if(s.type == Shape.CIRCLE) RayCast.rayCircle (r,s.circle );
			        else                       RayCast.rayPolygon(r,s.polygon,tempn);
			if(t<mint) {
				mint = t;
				mino = s.body;
				mins = s;
				vec_set(polyn,tempn.p);
			}
		});
		
		var retv;
		if(!(mint<ct||mint>nt) && mint!=RayCast.FAIL) {
			if(ret==null) ret = new RayResult();
			ret.obj = mino;
			ret.shape = mins;
			vec_set(ret.p,r.a);
			vec_addeq(ret.p,r.v,mint);
			if(ret.shape.type == Shape.CIRCLE) {
				vec_sub(ret.p,ret.shape.circle.centre.p,ret.n);
				vec_normalise(ret.n);
			}else {
				vec_set(ret.n,polyn);
			}
			ret.t = mint;
			retv = true;
		}else retv = false;
		
		retv;
	})
					return ret;
			}
			
			cu = nu;
			cv = nv;
			ct = nt;
		}
		
		return null;
	}
	
	
	
	public override function visitate() _visitate_( ) 
}
